/*
* Copyright 2011 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
* Software Foundation version 2 of the License.
*
* This program is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
* more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/

/**
 * @file   test_calibration.cpp
 * @author Ugo Cupcic <ugo@shadowrobot.com>
 * @date   Fri Jun 10 15:52:46 2011
 *
 * @brief This is a set of unit tests testing the calibration process.
 *
 *
 */

#include "sr_utilities/calibration.hpp"
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <vector>

class CalibrationTest
{
public:
  CalibrationTest()
  {
  }

  ~CalibrationTest()
  {
  }
};

///////////////////////
// TWO POINTS TABLES  //
///////////////////////

/**
 * We test that the value generated by a two point table
 * with a value asked before the table is correct:
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *
 * calibration(0) = 1.0
 */
TEST(Calibration2Points, before)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(0.0), 1.0);

  delete cal;
}

/**
 * We test that the value generated by a two point table
 * with a value asked in the table is correct:
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *
 * calibration(1.5) = 4.0
 */
TEST(Calibration2Points, in)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(1.5), 4.0);
  delete cal;
}

/**
 * We test that the value generated by a two point table
 * with a value asked after the table is correct:
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *
 * calibration(3) = 7.0
 */
TEST(Calibration2Points, after)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(3.0), 7.0);
  delete cal;
}

/**
 * We test that the value generated by a two point table
 * with a value asked on a point of the table is correct:
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *
 * calibration(3) = 7.0
 */
TEST(Calibration2Points, on)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(2.0), 5.0);
  EXPECT_EQ(cal->compute(1.0), 3.0);
  delete cal;
}



//////////////////////////
// THREE POINTS TABLES  //
////////////////////////

/**
 * We test that the value generated by a three point table
 * with a value asked before the table is correct:
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *  | 3.0 | 4.0 |
 *
 * calibration(0) = 1.0
 */
TEST(Calibration3Points, before)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);
  point.raw_value = 3.0;
  point.calibrated_value = 4.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(0.0), 1.0);
  delete cal;
}

/**
 * We test that the value generated by a three point table
 * with a value asked in the first interval is correct.
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *  | 3.0 | 4.0 |
 *
 * calibration(1.5) = 4.0
 */
TEST(Calibration3Points, first_interval)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);
  point.raw_value = 3.0;
  point.calibrated_value = 4.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(1.5), 4.0);
  delete cal;
}

/**
 * We test that the value generated by a three point table
 * with a value asked in the second interval is correct.
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *  | 3.0 | 4.0 |
 *
 * calibration(2.5) = 4.5
 */
TEST(Calibration3Points, second_interval)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);
  point.raw_value = 3.0;
  point.calibrated_value = 4.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(2.5), 4.5);
  delete cal;
}

/**
 * We test that the value generated by a three point table
 * with a value asked after the table is correct.
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *  | 3.0 | 4.0 |
 *
 * calibration(4.0) = 3.0
 */
TEST(Calibration3Points, after)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);
  point.raw_value = 3.0;
  point.calibrated_value = 4.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(4.0), 3.0);
  delete cal;
}

/**
 * We test that the value generated by a three point table
 * with a value asked on a point of the table is correct.
 * table:
 *  | 1.0 | 3.0 |
 *  | 2.0 | 5.0 |
 *  | 3.0 | 4.0 |
 *
 * calibration(2.0) = 5.0
 */
TEST(Calibration3Points, on)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;
  point.raw_value = 1.0;
  point.calibrated_value = 3.0;
  cal_table.push_back(point);
  point.raw_value = 2.0;
  point.calibrated_value = 5.0;
  cal_table.push_back(point);
  point.raw_value = 3.0;
  point.calibrated_value = 4.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(1.0), 3.0);
  EXPECT_EQ(cal->compute(2.0), 5.0);
  EXPECT_EQ(cal->compute(3.0), 4.0);
  delete cal;
}


/**
 * We test that the values generated by a five points table
 * with the table being initialised in the wrong order.
 * table:
 *  | 1.0 | -2.0 |
 *  | 2.0 |  0.0 |
 *  | 3.0 | -4.0 |
 *  | 5.0 |  6.0 |
 *  | 10. |  0.0 |
 *
 * calibration(0.0) = -4.0
 * calibration(1.0) = -2.0
 * calibration(1.5) = -1.0
 * calibration(2.0) =  0.0
 * calibration(2.5) = -2.0
 * calibration(3.0) = -4.0
 * calibration(4.0) =  1.0
 * calibration(5.0) =  6.0
 * calibration(7.5) = 3.0
 * calibration(10.) = 0.0
 * calibration(15.) = -6.0
 */
TEST(Calibration5Points, mixed_and_big_test)
{
  std::vector<joint_calibration::Point> cal_table;

  joint_calibration::Point point;

  point.raw_value = 5.0;
  point.calibrated_value = 6.0;
  cal_table.push_back(point);

  point.raw_value = 2.0;
  point.calibrated_value = 0.0;
  cal_table.push_back(point);

  point.raw_value = 1.0;
  point.calibrated_value = -2.0;
  cal_table.push_back(point);

  point.raw_value = 10.0;
  point.calibrated_value = 0.0;
  cal_table.push_back(point);

  point.raw_value = 3.0;
  point.calibrated_value = -4.0;
  cal_table.push_back(point);

  shadow_robot::JointCalibration *cal = new shadow_robot::JointCalibration(cal_table);

  EXPECT_EQ(cal->compute(0.0), -4.0);
  EXPECT_EQ(cal->compute(1.0), -2.0);
  EXPECT_EQ(cal->compute(1.5), -1.0);
  EXPECT_EQ(cal->compute(2.0), 0.0);
  EXPECT_EQ(cal->compute(2.5), -2.0);
  EXPECT_EQ(cal->compute(3.0), -4.0);
  EXPECT_EQ(cal->compute(4.0), 1.0);
  EXPECT_EQ(cal->compute(5.0), 6.0);
  EXPECT_EQ(cal->compute(7.5), 3.0);
  EXPECT_EQ(cal->compute(10.), 0.0);
  EXPECT_EQ(cal->compute(15.), -6.0);
  delete cal;
}



/////////////////////
//     MAIN       //
///////////////////

int main(int argc, char **argv)
{
  ros::init(argc, argv, "calibration_test");

  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
/* For the emacs weenies in the crowd.
   Local Variables:
   c-basic-offset: 2
   End:
*/
